#include "../include/Matrix.h"
#include <cmath>
#include <cassert>

#define EPSLON 1e-7

Matrix::Matrix(int _rows, int _columns)
{
    assert(_rows >=0 && _columns >=0);
    nRows = _rows;
    nColumns = _columns;
    data = new float*[nRows];
    for(int i=0; i < nRows; ++i)
    {
        data[i] = new float[nColumns];
        for(int j = 0; j < nColumns; ++j)
            data[i][j] = 0.0;
    }
}

Matrix::Matrix(const Matrix& m)
{
    nRows = m.rows();
    nColumns = m.columns();
    data = new float*[nRows];
    for(int i=0; i < nRows; ++i)
    {
        data[i] = new float[nColumns];
        for(int j = 0; j < nColumns; ++j)
            data[i][j] = m(i,j);
    }
}

Matrix::Matrix(const Vector& v)
{
    if(v.type() == LINEVECTOR)
    {
       nRows    = 1;
       nColumns = v.dim();
       data = new float*[nRows];
       data[0] = new float[nColumns];
       for(int j = 0; j < nColumns; ++j)
               data[0][j] = v(j);

    }else
    {
       nRows    = v.dim();
       nColumns = 1;
       data = new float*[nRows];
       for(int i=0; i < nRows; ++i)
       {
           data[i] = new float[nColumns];
           data[i][0] = v(i);
       }
    }

}

Matrix::~Matrix()
{
    for(int i = 0; i < nRows; ++i)
        delete [] data[i];
    delete [] data;
}

const float Matrix::operator()(int r, int c)const
{
    assert(r >=0 && r < nRows && c >=0 && c < nColumns);
    return data[r][c];
}

float& Matrix::operator()(int r, int c)
{
    assert(r >=0 && r < nRows && c >=0 && c < nColumns);
    return data[r][c];
}

void   Matrix::setRow   (int idx, const Vector& v)
{
    assert( idx >= 0 && idx < columns());
    assert( v.dim() == columns() );

    for(int i = 0; i < columns(); ++i)
            data[idx][i] = v(i);

}

void   Matrix::setColumn (int idx, const Vector& v)
{
    assert( idx >= 0 && idx < rows());
    assert( v.dim() == rows() );

    for(int i = 0; i < rows(); ++i)
            data[i][idx] = v(i);
}

void   Matrix::setVector (int idx, const Vector& v)
{
    if(v.type() == LINEVECTOR)
        return setRow(idx,v);
    else
        return setColumn(idx,v);
}

const int Matrix::rows()const
{
    return nRows;
}

const int Matrix::columns()const
{
    return nColumns;
}

Vector Matrix::row(int r)const
{
    assert(r >=0 && r < nRows);
    Vector v(nColumns);
    for(int i = 0; i < nColumns; ++i)
        v(i) = data[r][i];

    return v;
}

Matrix          Matrix::operator+  (const Matrix& m)const
{
    assert(rows() == m.rows() && columns() == m.columns());
    Matrix resp(rows(), columns());
    for(int i = 0; i < rows(); ++i)
        for(int j = 0; j < columns(); ++j)
            resp(i,j) = data[i][j] + m(i,j);
    return resp;
}

Matrix          Matrix::operator-  (const Matrix& m)const
{
    assert(rows() == m.rows() && columns() == m.columns());
    Matrix resp(rows(), columns());
    for(int i = 0; i < rows(); ++i)
        for(int j = 0; j < columns(); ++j)
            resp(i,j) = data[i][j] - m(i,j);
    return resp;
}
Matrix          Matrix::operator-  ()              const
{
    Matrix resp(rows(), columns());
    for(int i = 0; i < rows(); ++i)
        for(int j = 0; j < columns(); ++j)
            resp(i,j) = -data[i][j];
    return resp;
}
Matrix          Matrix::operator*  (float a)const
{
    Matrix resp(rows(), columns());
    for(int i = 0; i < rows(); ++i)
        for(int j = 0; j < columns(); ++j)
            resp(i,j) = data[i][j] * a;
    return resp;
}
Matrix                  operator*  (float a,const Matrix& m )
{
    return m*a;
}

const Matrix&  Matrix::operator=  (const Matrix& m)
{
    if(columns() != m.columns() || rows() != m.rows())
    {
        for(int i = 0; i < nRows; ++i)
            delete [] data[i];
        delete [] data;
        nRows    = m.rows();
        nColumns = m.columns();
        data = new float*[nRows];
        for(int i=0; i < nRows; ++i)
            data[i] = new float[nColumns];
    }

    for(int i=0; i < nRows; ++i)
    {
        for(int j = 0; j < nColumns; ++j)
            data[i][j] = m(i,j);
    }

    return *this;
}

Matrix Matrix::operator*  (const Matrix& m)const
{
    assert(columns() == m.rows());
    Matrix resp(rows(), m.columns());

    for(int i = 0; i < rows(); ++i)
    {
        for(int j = 0; j < m.columns(); ++j)
        {
            float s = 0;
            for(int k = 0; k < columns(); ++k)
                s += data[i][k] * m(k,j);
            resp(i,j) = s;
        }
    }

    return resp;
}

Vector  Matrix::operator*  (const Vector& v)const
{

    assert(columns() == v.dim() && v.type() == COLUMNVECTOR );
    Vector resp(rows());

    for(int i = 0; i < rows(); ++i)
    {
        float s = 0;
        for(int k = 0; k < columns(); ++k)
            s += data[i][k] * v(k);
        resp(i) = s;
    }

    return resp.transpose();
}

Vector  operator*  (const Vector& v,const Matrix& m)
{
    assert(m.rows() == v.dim() && v.type() == LINEVECTOR );
    Vector resp(m.columns());

    for(int i = 0; i < m.columns(); ++i)
    {
        float s = 0;
        for(int k = 0; k < m.rows(); ++k)
            s += v(k) * m(k,i);
        resp(i) = s;
    }

    return resp;
}

Vector Matrix::column(int c)const
{
    assert(c >=0 && c < nColumns);
    Vector v(nRows);
    for(int i = 0; i < nRows; ++i)
        v(i) = data[i][c];

    return v.transpose();
}

std::ostream& operator<<(std::ostream& out, const Matrix& m)
{
    out << " _";
    for(int i = 0; i < m.columns(); ++i)
        out << "\t";
    out << "\t_" << std::endl;

    for(int i = 0; i < m.rows()-1; ++i)
    {
        out << "|";
        for(int j = 0; j < m.columns(); ++j)
            out << "\t" << m(i,j);
        out << "\t |" << std::endl;
    }
    out << "|_";
    for(int j = 0; j < m.columns(); ++j)
        out << "\t" << m(m.rows()-1,j) ;
    out << "\t_|";

    return out;
}

void Matrix::set(float * d)
{
    for(int i = 0; i < rows(); ++i)
        for(int j = 0; j < columns(); ++j)
            data[i][j] = d[i*columns() + j];
}

void Matrix::set(float **d)
{
    for(int i = 0; i < rows(); ++i)
        for(int j = 0; j < columns(); ++j)
            data[i][j] = d[i][j];
}

Matrix Matrix::transpose()const
{
    Matrix resp(columns(), rows());
    for(int i = 0; i < columns(); ++i)
        for(int j = 0; j < rows(); ++j)
            resp(i,j) = data[j][i];

    return resp;
}

void Matrix::loadIdentity()
{
    for(int i = 0; i < rows(); ++i)
        for(int j = 0; j < columns(); ++j)
            if(i == j)
                data[i][j] = 1.0;
            else
                data[i][j] = 0.0;
}

void Matrix::LU_Decomposition(Matrix& L, Matrix& U)
{
    assert(rows() == columns());
    int n = rows();
    L = U = *this;
    L.loadIdentity();

    for(int i = 0; i < n-1; ++i)
    {
        for(int j = i+1; j < n; ++j)
        {
            float m = U(j,i)/ U(i,i);
            L(j,i) = m;
            U.setRow(j, U.row(j) - m*U.row(i));
        }
    }
}

void Matrix::swapLines(int lineA, int lineB)
{
    Vector v(row(lineA));
    setRow(lineA, row(lineB));
    setRow(lineB, v);
}

Vector Matrix::solveGauss(const Vector& b)
{
    assert(rows() == columns() && columns() == b.dim() && b.type() == COLUMNVECTOR);
    Matrix A = *this;
    Vector B = b;
    int n = rows();

    for(int i = 0; i < n-1; ++i)
    {
        int max = i;
        for(int j = i+1; j < n; ++j)
            max = fabs(A(i,j))>fabs(A(i,max))? j : max;

        if(max!=i)
        {
            A.swapLines(max, i);
            B.swap(max,i);
        }
        for(int j = i+1; j < n; ++j)
        {
            float m = A(j,i)/ A(i,i);
            A.setRow(j, A.row(j) - m*A.row(i));
            B(j) -= m*B(i);
        }
    }

    Vector X = B;

    for(int i = n-1; i >=0; --i)
    {
        X(i) /= A(i,i);
        for(int j = i-1; j>=0; --j)
        {
            X(j) -= X(i)*A(j,i);
        }
    }

    return X;
}

Vector Matrix::solveGaussJacob(const Vector& b)
{
    assert(rows() == columns() && columns() == b.dim() && b.type() == COLUMNVECTOR);

    Vector X1(b.dim()), X2(b.dim());

    X1(0) = (1.0);
    int cnt = 0;


    do{
        X2 = X1;

        for(int i= 0 ; i < rows(); ++i)
        {
            float s = 0.0;
            for(int j= 0; j < columns(); ++j)
                s += X2(j)*data[i][j];
            s -= X2(i)*data[i][i];

            X1(i) = (b(i) - s)/data[i][i];
        }

        cnt++;

    }while( (X1 - X2).length()/X1.length() > EPSLON  && cnt < 50);

    return X1;
}

Vector Matrix::solveGaussSeidel(const Vector& b)
{
    assert(rows() == columns() && columns() == b.dim() && b.type() == COLUMNVECTOR);

    Vector X1(b.dim()), X2(b.dim());

    X1(0) = (1.0);
    int cnt = 0;


    do{
        X2 = X1;

        for(int i= 0 ; i < rows(); ++i)
        {
            float s = 0.0;
            for(int j= 0; j < i; ++j)
                s += X1(j)*data[i][j];
            for(int j= i+1; j < columns(); ++j)
                s += X2(j)*data[i][j];
            X1(i) = (b(i) - s)/data[i][i];
        }

        cnt++;

    }while( (X1 - X2).length()/X1.length() > EPSLON  && cnt < 50);

    return X1;
}

Matrix Matrix::choleskyDecomposition()
{
    assert(rows() == columns());
    int n = rows();

    Matrix L(n,n);
    Matrix A = *this;

    double soma = 0;

    for (int j=0; j<n; j++)
    {
    // diagonal principal
        soma = 0;
        for(int i=0;i<j;i++)
        {
            soma=soma+ L(j,i)*L(j,i);
        }
        L(j,j)=sqrt(A(j,j)-soma);

        for(int i=j+1; i<n; i++)
        {
            // Sum l_i,p l_j,p
            soma = 0;
            for (int p = 0; p < j; p++)
            {
                soma = soma + L(i, p) * L(j, p);
            }

            L(i,j)= (A(i,j) - soma)/ L(j,j);
        }
    }

    return L;
}
